#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <string>
#include <iostream>

#include <fstream>

using namespace cv;
using namespace std;

double tailleUniteMire = 1;

///Calibrage intrinsèque

int main(int argc, char *argv[] )
{
//    cv::VideoCapture cap(std::string("mire1_4x3x27mm.avi"));
    cv::VideoCapture cap(std::string("mire1_4x3x27mm.avi"));
    if(!cap.isOpened())  // check if we succeeded
        return -1;

    int mirParX = atoi(argv[1]), mirParY = atoi(argv[2]);

//    cv::namedWindow("Video");
    cv::namedWindow("Pattern",CV_WINDOW_AUTOSIZE);

    vector<Point3f> v3f;
    for(int i = 0; i < 3; i++)
        for(int j = 0; j < 4; j++)
            v3f.push_back(Point3f(i,j,0));

    vector<vector<Point2f> > imagePoints;
    vector<vector<Point3f> > objectPoints;  //positions des coins de la mire dans l'espace

     Mat frame,img;
     int imgX=0,imgY=0;



    while(cap.grab() )
    {
//        cap >> frame; // get a new frame from camera
//        cap >> img;
        cap.retrieve(frame);

        char key = ' ';
        key =  cv::waitKey(50);

        if(imgX == 0) {imgX = frame.rows; imgY = frame.cols;}

        Size patternsize(mirParX,mirParY); //interior number of corners
        vector<Point2f> corners;           //this will be filled by the detected corners

        bool patternfound = findChessboardCorners(frame, patternsize, corners,
                CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE);

        objectPoints.push_back(v3f);
        imagePoints.push_back(corners);

        if(patternfound)
            drawChessboardCorners(frame, patternsize, Mat(corners), patternfound);

        cv::imshow( "Pattern",frame);

        if(key == 'q')
              break;
    }

    Mat cameraMatrix,distCoeffs;
    vector<Mat> rvecs, tvecs;

    bool coordIntrinsequesCalculees = false;

            calibrateCamera(objectPoints,imagePoints, Size(imgX,imgY), cameraMatrix,distCoeffs, rvecs, tvecs);
            cout << "Coordonnées calculées. " << endl;
            coordIntrinsequesCalculees = true;

            ofstream fCoordIntrin("intrinseques2.txt");

            if(fCoordIntrin)
            {
                fCoordIntrin << "distortion:" << endl;
                for(int i = 0; i < 4; i ++)
                        fCoordIntrin <<  distCoeffs.at<double>(0,i) << " ";

                fCoordIntrin << endl << "projection:";

                for(int r = 0; r < cameraMatrix.rows ; r++)
                {
                    fCoordIntrin << endl;
                    for(int c = 0; c < cameraMatrix.cols ; c++)
                        fCoordIntrin <<   cameraMatrix.at<double>(r,c) << " ";
                }

                cout << "Coordonnees intrinseques sauvegardees dans intrinseques.txt." << endl;
            }
//        }


//
//
//    while(true)
//    {
//        char key = ' ';
//        key = cv::waitKey(100);
//
//        if(key == 'c')
//        {
//            calibrateCamera(objectPoints,imagePoints, Size(imgX,imgY), cameraMatrix,distCoeffs, rvecs, tvecs);
//            cout << "Coordonnées calculées. " << endl;
//            coordIntrinsequesCalculees = true;
//        }
//        else if (key == 'd' && coordIntrinsequesCalculees)
//        {
//            cout << "Camera matrix: " << endl;
//            for(int r = 0; r < cameraMatrix.rows ; r++)
//            {
//                cout << endl;
//                for(int c = 0; c < cameraMatrix.cols ; c++)
//                    cout <<   cameraMatrix.at<double>(r,c) << " ";
//            }
//
//            cout << "Valeurs distorsion: " << endl;
//            cout << "k1 " << distCoeffs.at<double>(0,0) << " k2 " << distCoeffs.at<double>(0,1)
//                << " p1 " << distCoeffs.at<double>(0,2) << " p2 " << distCoeffs.at<double>(0,3) << endl;
//        }
//        else if (key == 's' && coordIntrinsequesCalculees)
//        {
////            ofstream fCoordIntrin("intrinseques.txt");
//            ofstream fCoordIntrin("intrinseques2.txt");
//
//            if(fCoordIntrin)
//            {
//                fCoordIntrin << "distortion:" << endl;
//                for(int i = 0; i < 4; i ++)
//                        fCoordIntrin <<  distCoeffs.at<double>(0,i) << " ";
//
//                fCoordIntrin << endl << "projection:";
//
//                for(int r = 0; r < cameraMatrix.rows ; r++)
//                {
//                    fCoordIntrin << endl;
//                    for(int c = 0; c < cameraMatrix.cols ; c++)
//                        fCoordIntrin <<   cameraMatrix.at<double>(r,c) << " ";
//                }
//
//                cout << "Coordonnees intrinseques sauvegardees dans intrinseques.txt." << endl;
//            }
//        }

//        if(key == 'q')
//              break;
//    }
    cout << endl;
    return 0;
}
